import rclpy
from rclpy.node import Node
from agv_srvs.srv import ControllerEnable


class ControllerEnableServer(Node):
    def __init__(self):
        super().__init__('controller_enable_server')
        self.service = self.create_service(
            ControllerEnable, '/controller_enable', self.handle_enable_controllers)
        # self.service.wait_for_service()
        self.get_logger().info('ControllerEnable service ready.')

    def handle_enable_controllers(self, request, response):
        self.get_logger().info(f'Received request to {"enable" if request.enable else "disable"} controllers: {request.controller_names}')
        # 在这里添加实际的启用/禁用控制器的逻辑
        # 例如，调用某个 API 或执行某些操作
        # 这里为了示例，我们总是返回成功
        response.success = True
        return response


def main(args=None):
    rclpy.init(args=args)
    controller_enable_server = ControllerEnableServer()
    rclpy.spin(controller_enable_server)
    controller_enable_server.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()